{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0",
   "metadata": {},
   "source": [
    "# Head control"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1",
   "metadata": {},
   "source": [
    "Reachy 2's head is mounted on an Orbita3D actuator, referred to as the **neck** actuator, giving 3 degrees of freedom to control the head orientation.  \n",
    "\n",
    "Before starting to control the head, connect to your Reachy and turn it on."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2",
   "metadata": {},
   "source": [
    "## Initialize your robot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3",
   "metadata": {},
   "source": [
    "First connect to your robot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4",
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk import ReachySDK\n",
    "\n",
    "reachy = ReachySDK(host='localhost')  # Replace with the actual IP address"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5",
   "metadata": {},
   "source": [
    "Let's check what contains the head part:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.joints"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7",
   "metadata": {},
   "source": [
    "As you can see, the head is composed of **five joints**:\n",
    "- neck.roll\n",
    "- neck.pitch\n",
    "- neck.yaw\n",
    "- l_antenna\n",
    "- r_antenna\n",
    "\n",
    "The 3 first joints belong to the same Orbita3D actuator, referred to as the **neck**.\n",
    "\n",
    "To start, we need to turn the head on:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.turn_on()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9",
   "metadata": {},
   "source": [
    "You could of course turn on the whole robot by calling `reachy.turn_on()` directly."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10",
   "metadata": {},
   "source": [
    "## Head goto"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "11",
   "metadata": {},
   "source": [
    "There are several ways to control the head movements:\n",
    "- using the `look_at()`, `goto` and `rotate_by` methods, called directly at the **head** level. These methods work as in the [goto_introduction](2_goto_introduction.ipynb) example\n",
    "- controlling the joints goal positions, namely **reachy.head.neck.roll**, **reachy.head.neck.pitch** and **reachy.head.neck.yaw**."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12",
   "metadata": {},
   "source": [
    "### look_at()\n",
    "\n",
    "You can use the `look_at()` function to make the head look at a specific point in space. This point must be given in Reachy 2's coordinate system in **meters**. The coordinate system is the one we have seen previously:\n",
    "\n",
    "* the X axis corresponds to the forward arrow,\n",
    "* the Y axis corresponds to the right to left arrow,\n",
    "* the Z axis corresponds to the up arrow.\n",
    "\n",
    "The origin of this coordinate system is located in the upper part of the robot trunk.\n",
    "\n",
    "If you want Reachy to look forward, you can send it the following."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.look_at(x=0.5, y=0, z=0.2, duration=1.0, wait = True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14",
   "metadata": {},
   "source": [
    "You can use multiple *look_at* to chain head movements, or even chain them with the `goto()` function described below. As seen in the goto tutorial, the commands on the head will be stacked.\n",
    "For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "look_right = reachy.head.look_at(x=0.5, y=-0.3, z=0.1, duration=1.0)\n",
    "look_down = reachy.head.look_at(x=0.5, y=0, z=-0.4, duration=1.0)\n",
    "look_left = reachy.head.look_at(x=0.5, y=0.3, z=-0.1, duration=1.0)\n",
    "look_front = reachy.head.look_at(x=0.5, y=0, z=0, duration=1.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "16",
   "metadata": {},
   "source": [
    "The best way to understand how to use the *look_at* is to play with it. Picture a position you would like Reachy's head to be in, guess a point which could match for the *look_at* and check if you got it right!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17",
   "metadata": {},
   "source": [
    "### goto()\n",
    "\n",
    "The `goto()` function is another way to control the head. There is two ways to use it :\n",
    "- from the desired orientation as a quaternion (in cartesian space)\n",
    "- from joints positions (in joint space)\n",
    "\n",
    "So, you give either a quaternion or a list of 3 degree values.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "19",
   "metadata": {},
   "source": [
    "#### In cartesian space\n",
    "\n",
    "You can control the head with a quaternion, in cartesian space.\n",
    "\n",
    "You can use [pyquaternion library](https://kieranwynn.github.io/pyquaternion/) to create suitable quaternion for this method."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "20",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pyquaternion import Quaternion\n",
    "\n",
    "q = Quaternion(axis=[1, 0, 0], angle=3.14159265 / 4) # tilt head about 45° to the right\n",
    "reachy.head.goto(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "64afc46e",
   "metadata": {},
   "source": [
    "#### In joint space\n",
    "\n",
    " You can directly control the joint value of the neck, giving the roll, pitch and yaw angles in degrees. The rotation is made in the order: roll, pitch, yaw, in the Orbita3D coordinate system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "624f4dfe",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.goto([15, -20, 0], duration=1.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b588f71",
   "metadata": {},
   "source": [
    "### Rotate_by()\n",
    "\n",
    "You can also control the head from its current position, by using the *rotate_by* function and specifying angular degree values in roll, pitch, yaw, either in Reachy's or head's frame. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d23ff45f",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.rotate_by(roll=0, pitch=0, yaw=20, frame='head')\n",
    "\n",
    "reachy.head.rotate_by(roll=-30, pitch=0, yaw=0, frame='robot')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "21",
   "metadata": {},
   "source": [
    "## Read head position\n",
    "\n",
    "You can read the head positions using : \n",
    "\n",
    "- Cartesian space : \n",
    "> `get_current_orientation()` will give the orientation as a quaternion\n",
    "\n",
    "\n",
    "- Joint space :  \n",
    "> `get_current_positions()` will give the neck's roll, pitch and yaw present_position\n",
    "\n",
    "### In cartesian space :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "22",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = reachy.head.get_current_orientation()\n",
    "print(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23",
   "metadata": {},
   "source": [
    "### In joint space : \n",
    "\n",
    "In case you feel more comfortable using roll, pitch, yaw angles rather than working with quaternions, you can retrieve those values from the **neck joints**."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.goto([20, 20, -10], wait = True)\n",
    "\n",
    "reachy.head.get_current_positions()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29",
   "metadata": {},
   "source": [
    "Then, you can reset the head to default position and turn it off."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.goto_posture(common_posture=\"default\", wait=True)\n",
    "    \n",
    "reachy.turn_off_smoothly()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5b478c4e",
   "metadata": {},
   "source": [
    "## Antennas goto"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "12b1a208",
   "metadata": {},
   "source": [
    "The antennas can be accessed through the head, using `reachy.head.l_antenna` and `reachy.head.r_antenna`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0af15dc0",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(reachy.head.r_antenna)\n",
    "print(reachy.head.l_antenna)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f45b3140",
   "metadata": {},
   "source": [
    "The methods to control the antennas are quite close to the one available for the head:\n",
    "- using the `goto` methods, which works like in the [goto_introduction](2_goto_introduction.ipynb) example\n",
    "- controlling the joint goal positions, namely **reachy.head.r_antenna** and **reachy.head.l_antenna**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c585af31",
   "metadata": {},
   "source": [
    "Turn on the head in order to be able to control the antennas:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b1c5c4b3",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.turn_on()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0e6e75f3",
   "metadata": {},
   "source": [
    "You can now send goto commands to the antennas:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e9c33233",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.head.r_antenna.goto(20, duration=0.5)\n",
    "reachy.head.l_antenna.goto(-20, duration=0.5)\n",
    "reachy.head.r_antenna.goto(0, duration=0.5)\n",
    "reachy.head.l_antenna.goto(0, duration=0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6fcd59de",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.turn_off_smoothly()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
